/*
 * SharpSensors.c
 *
 *  Created on: 2012.11.29.
 *      Author: AnnaBaba
 */

#include "SharpSensors.h"

__IO uint16_t SharpSensorValues[3];
float SharpSensorDistances[3];
float SharpSensorVoltages[3];
float dist1;

void SS_Pins_Init(void){
	GPIO_InitTypeDef GPIO_InitStructure;

	GPIO_StructInit(&GPIO_InitStructure);

	/* Configure ADC Channel1,2,3 pin as analog input ******************************/
	GPIO_InitStructure.GPIO_Pin = SS_LS_PIN | SS_RS_PIN | SS_FS_PIN;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL ;
	GPIO_Init(SS_SS_PORT, &GPIO_InitStructure);
}

void SS_Init(void){
	SS_Pins_Init();
	//ADC1:FrontSensor, ADC2:RightSensor, ADC3:LeftSensor from front
	if(!adc_inited)ADC_Initialize();
}

void SS_GetValues(void){
	uint8_t i;
	float volt;
	ADC_ChangeChannel(ADC_Channel_11, ADC_Channel_14, ADC_Channel_12);
	ADC_SetDMATarget(DMA2_Stream0, (uint32_t)&SharpSensorValues[0]);
	ADC_SetDMATarget(DMA2_Stream2, (uint32_t)&SharpSensorValues[1]);
	ADC_SetDMATarget(DMA2_Stream1, (uint32_t)&SharpSensorValues[2]);

	DMA_ClearFlag(DMA2_Stream1, DMA_FLAG_TCIF1);
	ADC_SoftwareStartConv(ADC1);
	ADC_SoftwareStartConv(ADC2);
	ADC_SoftwareStartConv(ADC3);
	while(DMA_GetFlagStatus(DMA2_Stream1, DMA_FLAG_TCIF1) == RESET);	//waiting for DMA transfer from ADC3
	for (i = 0; i < 3; i++){
		volt = (SharpSensorValues[i]>>4) * 3.0 / 0xfff;
		SharpSensorVoltages[i] = (SharpSensorValues[i]>>4) * 3.0 / 0xfff;
		if (volt > 2.6 || volt < 0.3)
			SharpSensorDistances[i] = 0xffff;
		else{
			if (!i){
				SharpSensorDistances[i] = -7.997 * volt * volt * volt * volt * volt + 68.629 * volt * volt * volt * volt - 238.88 * volt * volt * volt + 431.95 * volt * volt - 436.26 * volt + 242.15;
			}
			else{
				SharpSensorDistances[i] = -2.8663 * volt * volt * volt * volt * volt + 26.576 * volt * volt * volt * volt - 95.406 * volt * volt * volt + 168.2 * volt * volt - 153.25 * volt + 68.684;
			}
		}
	}

}


